# pyb类库
import pyb
from pyb import LED, Timer, UART
from machine import I2C,Pin
import utime
import time
# 外设库
import ps2
import encoder
import pid
import motor
import serial
import mpu6050
from oled import SSD1306_I2C
# 核心控制代码库
from maincontrol import *

def oled_display(ps_info, port_info, accel_tuple, encoder_tuple, speed_tuple):
    '''
    函数功能：OLED屏(128*64)显示运动状态参数
    入口参数：ps手柄状态，左右编码器值元组，左右轮速度元组
    '''
    oled.fill(0)
    # 静态字符
    # oled.text("count:",0,0)
    oled.text("X:",0,0); oled.text("Y:",43,0); oled.text("Z:",85,0); 
    oled.text("PS2:",0,10); oled.text("UART:",65,10);
    oled.text("Enc L:    R:",0,20)
    oled.text("SPEED:",0,30); oled.text("m/s",100,30); 
    
    # 动态字符
    oled.text(str(round(accel_tuple[0], 1)),13,0); oled.text(str(round(accel_tuple[1], 1)),56,0); oled.text(str(round(accel_tuple[2], 1)),98,0);
    oled.text(str(ps_info),35,10); oled.text(str(port_info),104,10); 
    oled.text(str(encoder_tuple[0]),45,20); oled.text(str(encoder_tuple[1]),95,20);
    oled.text(str(round(speed_tuple[0], 3)),50,30)
    oled.show()

def mainTimer_cb(cb):
    '''
    函数功能：main函数的定时器中断函数，用于总体程序逻辑的定时执行
    修改：修改对应的程序时间，以及程序的功能标志位(flag)
    '''
    global count, reset_flag, ps2_flag, enc_flag, pid_flag, toggle_flag, display_flag, serial_flag, accel_flag
    LED(3).toggle()
    count += 1
    if (count%1000) == 0: # 重置计数 1s
        reset_flag = True
    if (count%5) == 0: # 获取PS2手柄的值 5ms  
        ps2_flag = True   
        if (count%10) == 0: # pid刷新并控制电机 10ms
            pid_flag = True
            if (count%40) == 0: # oled刷新 40ms
                display_flag = True    
        if (count%15) == 0: # 刷新mpu6050 10ms
            accel_flag = True
            if (count%30) == 0: # 读取串口信息 30ms
                serial_flag =True     
        if (count%500) == 0: # LED闪烁 500ms
            toggle_flag = True
            
if __name__ == '__main__':
    # 初始化程序标志位
    count = 0 # 程序定时计数初值
    display_flag = False
    reset_flag = False
    toggle_flag = False
    enc_flag = False
    ps2_flag = False
    pid_flag = False
    serial_flag = False
    accel_flag = False
    # ps2手柄标志位
    joy_mode = False
    key_mode = False
    pwm_range = 100 # PWM的范围（0-100）,默认值，不要改动
    encoder_freq = 200 # 编码器读取频率
    
    # pid参数
    kp = 2.5
    ki = 0.3
    kd = 0

#-----------------------外设初始化--------------------------#
    # 定时器初始化：主程序逻辑控制，频率为1000Hz，每次计时1ms
    mainTimer = Timer(1, freq=1000, callback=mainTimer_cb) 
    print("定时器初始化----")
    # MPU6050初始化
    i2c = I2C(scl=Pin("X9"), sda=Pin("X10"))
    accel = mpu6050.accel(i2c)
    accel_dict = accel.processed_values()
    accel_tuple = (accel_dict["AcX"], accel_dict["AcY"], accel_dict["AcZ"], accel_dict["GyX"], accel_dict["GyY"], accel_dict["GyZ"])
    print("MPU6050初始化----")
    # OLED初始化
    i2c = I2C(scl = Pin("Y9"),sda = Pin("Y10"),freq = 400000) # 软件I2C
    oled = SSD1306_I2C(128, 64, i2c) #创建oled对象
    print("OLED初始化----")
    # PS2手柄初始化
    ps = ps2.PS2KEY('X18','X19','X20','X21')
    ps_info = "OFF"  # ps_info 读取PS2手柄连接状态
    print("PS2手柄初始化----")
    # 上下位机串口初始化
    uart = UART(2, 115200, timeout=10)
    ser = serial.Serial(uart)
    print("串口通信初始化----")
    port_info = "OFF"
    # 编码器初始化
    enc = encoder.Encoder(encoder_freq)
    print("编码器初始化----")
    # 初始化PID控制器
    pid = pid.PID(pwm_range, kp, ki, kd)
    print("PID初始化----")
    # 初始化电机
    motor = motor.Motor(pwm_range)
    print("电机初始化----")
    # 初始化运动控制
    psdriver = Ps2_Driver(enc, pid, motor)
    psdriver.ps2_driver_init()
    print("主控逻辑初始化----")
#-----------------------逻辑循环--------------------------#
    while True:        
        # 重置计数器 1s
        if reset_flag == True:
            count = 0
            reset_flag = False
        
        # 读取PS2手柄 5ms
        if ps2_flag == True:
            joy_data = ps2.data
            key = ps.ps2_key()
            joy_mode = True if (joy_data[1] == 185) else False # 摇杆标志位
            key_mode = True if (joy_data[1] == 160) else False # 按键标志位
            ps_info = "Key" if (key_mode == True) else "Joy" # 如果按键模式或者是摇杆模式开启

            if key_mode == True: # 按键模式
                target_speed_A, target_speed_B = psdriver.ps2_keycontrol(key)
                key_mode = False
            elif joy_mode == True: # 摇杆模式
                target_speed_A, target_speed_B = psdriver.ps2_joycontrol(joy_data)
                joy_mode = False
            # print("target_speed_A:", target_speed_A)
            # print("target_speed_B:", target_speed_B)
            ps2_flag = False
        
        # 主控逻辑刷新 10ms
        if pid_flag == True:
            enc_A = enc.encoder_A
            enc_B = enc.encoder_B
            target_enc_A = enc.target_enc_process(target_speed_A)
            target_enc_B = enc.target_enc_process(target_speed_B)
            # print("target_enc_A:", target_enc_A)
            # print("target_enc_B:", target_enc_B)
            psdriver.driver(target_enc_A, target_enc_B, enc_A, enc_B) # PS2手柄控制
            pid_flag = False
            speed_A = enc.get_speed_A()
        
        # mpu6050
        if accel_flag == True:
            try:
                accel_dict = accel.processed_values()
                accel_tuple = (accel_dict["AcX"], accel_dict["AcY"], accel_dict["AcZ"], accel_dict["GyX"], accel_dict["GyY"], accel_dict["GyZ"])
            except OSError:
                accel_tuple = (0,0,0,0,0,0)
            accel_flag = False
            # print(accel_tuple)

        # UART串口通信 30ms
        if serial_flag == True:
            zip_data = ser.format_data(encoders=(enc_A, enc_B), imu=accel_tuple)
            ser.send_data(zip_data)
            rec_data = ser.rev_data()
            port_info = "OFF" if (ser.serial_check(rec_data) == False) else "ON" # 判断串口是否接收到数据
            serial_flag = False
            # USB串口通信，便于调参
            # print(str(((enc_A, enc_B), accel_tuple, target_enc_A)) + "\n") # 
            print(str(((enc_A, enc_B), target_enc_A)) + "\n")  # PID调参

        # oled刷新 40ms
        if display_flag == True:
            oled_display(ps_info, port_info, accel_tuple, (enc_A, enc_B), (speed_A, 0)) 
            display_flag = False
        
        # LED闪烁 500ms
        if toggle_flag == True:
            LED(2).toggle() 
            toggle_flag = False

        